#include "RungeKutta.h"

#ifdef RUNGEKUTTA_MINATO
void RungeKutta4ordem::integra(SPHERE *s)
{						// Integração temporal: Método de Runge-Kutta de 4º ordem
    if ((s->Step & 2) == 0) {
        if ((s->Step & 1) == 0) {

            s->K1[0] = H * s->V[0];
            s->K1[1] = H * s->V[1];
            s->K1[2] = H * s->V[2];

            s->L1[0] = H * s->F[0] / s->Mass;
            s->L1[1] = H * s->F[1] / s->Mass;
            s->L1[2] = H * s->F[2] / s->Inertia;

            s->R[0] += 0.5 * s->K1[0];
            s->R[1] += 0.5 * s->K1[1];
            s->R[2] += 0.5 * s->K1[2];

            s->V[0] += 0.5 * s->L1[0];
            s->V[1] += 0.5 * s->L1[1];
            s->V[2] += 0.5 * s->L1[2];
        }
        else {
            s->K[0] = H * s->V[0];
            s->K[1] = H * s->V[1];
            s->K[2] = H * s->V[2];

            s->L[0] = H * s->F[0] / s->Mass;
            s->L[1] = H * s->F[1] / s->Mass;
            s->L[2] = H * s->F[2] / s->Inertia;

            s->R[0] += 0.5 * (s->K[0] - s->K1[0]);
            s->R[1] += 0.5 * (s->K[1] - s->K1[1]);
            s->R[2] += 0.5 * (s->K[2] - s->K1[2]);

            s->V[0] += 0.5 * (s->L[0] - s->L1[0]);
            s->V[1] += 0.5 * (s->L[1] - s->L1[1]);
            s->V[2] += 0.5 * (s->L[2] - s->L1[2]);
        }
    }
    else {
        if ((s->Step & 1) == 0) {

            s->K[0] = H * s->V[0] - 0.5 * s->K[0];
            s->K[1] = H * s->V[1] - 0.5 * s->K[1];
            s->K[2] = H * s->V[2] - 0.5 * s->K[2];

            s->L[0] = H * s->F[0] / s->Mass - 0.5 * s->L[0];
            s->L[1] = H * s->F[1] / s->Mass - 0.5 * s->L[1];
            s->L[2] = H * s->F[2] / s->Inertia - 0.5 * s->L[2];

            s->R[0] += s->K[0];
            s->R[1] += s->K[1];
            s->R[2] += s->K[2];

            s->V[0] += s->L[0];
            s->V[1] += s->L[1];
            s->V[2] += s->L[2];
        }
        else {
            s->R[0] += ( s->K1[0] - 4 * s->K[0] + H * s->V[0] ) / 6.0;
            s->R[1] += ( s->K1[1] - 4 * s->K[1] + H * s->V[1] ) / 6.0;
            s->R[2] += ( s->K1[2] - 4 * s->K[2] + H * s->V[2] ) / 6.0;

            s->V[0] += ( s->L1[0] - 4 * s->L[0] + H * s->F[0] / s->Mass ) / 6.0;
            s->V[1] += ( s->L1[1] - 4 * s->L[1] + H * s->F[1] / s->Mass ) / 6.0;
            s->V[2] += ( s->L1[2] - 4 * s->L[2] + H * s->F[2] / s->Inertia ) / 6.0;

            while (s->R[2] > CONST_PI) s->R[2] -= CONST_2_PI;
            while (s->R[2] < -CONST_PI) s->R[2] += CONST_2_PI;
        }
    }

    s->F[0] = s->F[1] = s->F[2] = 0.0;
}

void RungeKutta4ordem::calcular()
{						// Integração temporal: Método de Runge-Kutta de 4º ordem
    #pragma omp single
	{
        if (SPHERE::Step == 0) H = 4.0 * pModel->Precision;
        for (SPHERE* s = pModel->SPHList.Begin; s != NULL; s = s->Next) integra(s);
        SPHERE::Step++;
        if (SPHERE::Step == 4) SPHERE::Step = 0;
        if (SPHERE::Step & 1) pModel->Time += (pModel->Precision + pModel->Precision);
	}
    #pragma omp barrier
}

void RungeKutta4ordemParalelo::calcular()
{				// Integração temporal: Método de Runge-Kutta de 4º ordem
	#pragma omp single
	{
        if (SPHERE::Step == 0) H = 4.0 * pModel->Precision;
	}
	#pragma omp barrier
    #pragma omp for schedule (dynamic,1)
    for (int i = 0; i < pModel->numProceEsf; i++)
    {
        SPHERE* testeFim;
        if (i!=pModel->numProceEsf-1) testeFim = pModel->threads[i+1];
        else testeFim=NULL;

        for (SPHERE* s = pModel->threads[i]; s != testeFim; s = s->Next) integra(s);
    }
    #pragma omp barrier
    #pragma omp single
	{
        SPHERE::Step++;
        if (SPHERE::Step == 4) SPHERE::Step = 0;
        if (SPHERE::Step & 1) pModel->Time += (pModel->Precision + pModel->Precision);
	}
    #pragma omp barrier
}

#endif

